#include "include.h"

#define G 9.801

static const uint16_t tb_atan[256] = {
0, 229, 458, 687, 916, 1145, 1374, 1603,
1832, 2061, 2290, 2519, 2748, 2976, 3205, 3433,
3662, 3890, 4118, 4346, 4574, 4802, 5029, 5257,
5484, 5711, 5938, 6165, 6391, 6618, 6844, 7070,
7296, 7521, 7746, 7971, 8196, 8421, 8645, 8869,
9093, 9317, 9540, 9763, 9986, 10208, 10431, 10652,
10874, 11095, 11316, 11537, 11757, 11977, 12197, 12416,
12635, 12853, 13071, 13289, 13507, 13724, 13940, 14157,
14373, 14588, 14803, 15018, 15232, 15446, 15660, 15873,
16085, 16297, 16509, 16720, 16931, 17142, 17352, 17561,
17770, 17979, 18187, 18394, 18601, 18808, 19014, 19220,
19425, 19630, 19834, 20038, 20241, 20444, 20646, 20848,
21049, 21250, 21450, 21649, 21848, 22047, 22245, 22443,
22640, 22836, 23032, 23227, 23422, 23616, 23810, 24003,
24196, 24388, 24580, 24771, 24961, 25151, 25340, 25529,
25717, 25905, 26092, 26278, 26464, 26649, 26834, 27018,
27202, 27385, 27568, 27750, 27931, 28112, 28292, 28471,
28650, 28829, 29007, 29184, 29361, 29537, 29712, 29887,
30062, 30236, 30409, 30582, 30754, 30925, 31096, 31266,
31436, 31605, 31774, 31942, 32109, 32276, 32442, 32608,
32773, 32938, 33101, 33265, 33428, 33590, 33751, 33913,
34073, 34233, 34392, 34551, 34709, 34867, 35024, 35180,
35336, 35492, 35646, 35801, 35954, 36107, 36260, 36412,
36563, 36714, 36864, 37014, 37163, 37312, 37460, 37607,
37754, 37901, 38047, 38192, 38337, 38481, 38624, 38768,
38910, 39052, 39194, 39335, 39475, 39615, 39754, 39893,
40032, 40169, 40307, 40443, 40580, 40715, 40850, 40985,
41119, 41253, 41386, 41519, 41651, 41782, 41913, 42044,
42174, 42303, 42432, 42561, 42689, 42817, 42944, 43070,
43196, 43322, 43447, 43572, 43696, 43819, 43943, 44065,
44188, 44309, 44431, 44551, 44672, 44792, 44911, 45030,
45148, 45266, 45384, 45501, 45618, 45734, 45849, 45965,
};

/*
int16_t fast_atan2(float real, float imag)
{
    float abs_real;
    float abs_imag;
    int16_t phase;
    uint16_t zone;
    uint32_t tmp_tan;
    uint32_t delta_tan;
    if (real < 0)
    {
       abs_real = -real;
       zone = 1U;
    }
    else
    {
       abs_real = real;
       zone = 0U;
    }
    if (imag < 0)
    {
       abs_imag = -imag;
       zone += 2U;      
    }
    else
    {
       abs_imag = imag; 
       zone = 0U;       
    }
    if (abs_imag <= abs_real) 
    {
       tmp_tan = (uint32_t)abs_imag<<8U;
       tmp_tan /= abs_real;
       phase = tb_atan[tmp_tan]>>3U; 
    }
    else if ((abs_imag>>7U) <= abs_real) 
    {
       delta_tan = ((uint32_t)(abs_imag - abs_real))<<15U;
       tmp_tan = (uint32_t)abs_imag + (uint32_t)abs_real;
       delta_tan /= tmp_tan;
       delta_tan >>= 7U;
       phase = tb_atan[delta_tan]>>3U; 
       phase += 5760; 
    }
   else 
    {
       phase = 11520;
    }
    switch (zone)
    {
       case 0U:
       {
           break; 
       }
       case 1U:
       {
           phase = 23040 - phase;
           break; 
       }
       case 2U:
       {
           phase = -phase;
           break; 
       }
       case 3U:
       {
           phase = phase - 23040;
           break; 
       }
       default:
       {
           break;
       }
    }
   return(phase);
}
*/

/*#define FAST_ATAN2_PIBY2_FLOAT  1.5707963f
float fast_atan2(float y, float x)
{
  if (x == 0.0f) {
    if (y > 0.0f) {
      return FAST_ATAN2_PIBY2_FLOAT;
    }
    if (y == 0.0f) {
      return 0.0f;
    }
    return -FAST_ATAN2_PIBY2_FLOAT;
  }
  float atan;
  float z = y/x;
  if (fabs( z ) < 1.0f) {
    atan = z / (1.0f + 0.28f * z * z);
    if (x < 0.0f) {
      if (y < 0.0f) {
        return atan - PI;
      }
      return atan + PI;
    }
  } else {
    atan = FAST_ATAN2_PIBY2_FLOAT - (z / (z * z + 0.28f));
    if (y < 0.0f) {
      return atan - PI;
    }
  }
  return atan;
}*/

void update_attitude(BALANCE_CONTROL_t *balance_control)
{
    balance_control->Acc_Z = Get_Z_Acc() *G / (32767.0 / 4);  //Acc range -4g~+4g int16
    balance_control->Acc_X = Get_X_Acc() *G / (32767.0 / 4);  //Acc range -4g~+4g int16
    balance_control->Gyro_Y = Get_Y_Gyro()/ (32767.0 / 1000.0);  //gyro range -1000~+1000 deg/s int16
    balance_control->Gyro_Y -= balance_control->Gyro_Y_zero;
    //ACC(m/s) Gyro(deg/s)
}

void ZeroDrift(BALANCE_CONTROL_t *balance_control)
{
   double Gyro_Y_zero = 0;
   for(int i=0;i<500;i++)
   {
      balance_control->f_update_attitude(balance_control);
      Gyro_Y_zero += balance_control->Gyro_Y;
      DELAY_MS(5);
   }
   balance_control->Gyro_Y_zero = Gyro_Y_zero / 500;
}

void init_balance_control(BALANCE_CONTROL_t *balance_control)
{
    PID_StructInit(&(balance_control->balance_control_pid));
    balance_control->balance_control_pid.f_ParamInit(
        &(balance_control->balance_control_pid),
        BALANCE_CONTROL_P,
        BALANCE_CONTROL_I,
        BALANCE_CONTROL_D,
        BALANCE_CONTROL_INTEGRAL_LIMIT,
        BALANCE_CONTROL_MAX_OUTPUT);
    balance_control->f_ZeroDrift(balance_control);
}

void BALANCE_CONTROL_StructInit(BALANCE_CONTROL_t *balance_control)
{
    balance_control->f_init_balance_control = init_balance_control;
    balance_control->f_update_attitude = update_attitude;
    balance_control->f_ZeroDrift = ZeroDrift;
    balance_control->Acc_Z = 0;
    balance_control->Gyro_Y = 0;
    balance_control->Gyro_Y_zero = 0;
    balance_control->balance_control_target_angle = BALANCE_CONTROL_TARGET_ANGLE;
    balance_control->balance_control_k = BALANCE_CONTROL_K;
}